
/******************************************************************************
 * MODULE     : curve.cpp
 * DESCRIPTION: mathematical curves
 * COPYRIGHT  : (C) 2003  Joris van der Hoeven and Henri Lesourd
 *******************************************************************************
 * This software falls under the GNU general public license version 3 or later.
 * It comes WITHOUT ANY WARRANTY WHATSOEVER. For details, see the file LICENSE
 * in the root directory or <http://www.gnu.org/licenses/gpl-3.0.html>.
 ******************************************************************************/

#include "curve.hpp"
#include "basic.hpp"
#include "equations.hpp"
#include "frame.hpp"
#include "math_util.hpp"
#include "merge_sort.hpp"
#include "path.hpp"
#include "polynomial.hpp"

/******************************************************************************
 * General routines
 ******************************************************************************/

array<point>
curve_rep::rectify (double eps) {
  array<point> a (1);
  a[0]= evaluate (0.0);
  rectify_cumul (a, eps);
  return a;
}

double
curve_rep::bound (double t, double eps) {
  // TODO: Improve this, as soon as the curvature ()
  //       for transformed_curves will be implemented
  bool   b;
  double ng= norm (grad (t, b));
  if (ng <= 1.0e-12) return tm_infinity;
  double delta= eps / ng;
  while (delta >= 1.0e-6) {
    point val1= evaluate (t);
    point val2= evaluate (max (t - delta, 0.0));
    point val3= evaluate (min (t + delta, 1.0));
    if (norm (val2 - val1) <= eps + 1.0e-6 &&
        norm (val3 - val1) <= eps + 1.0e-6)
      return delta;
    delta/= 2.0;
  }
  return delta;
}

int
curve_rep::get_control_points (array<double>& abs, array<point>& pts,
                               array<path>& cip) {
  abs= array<double> ();
  pts= array<point> ();
  cip= array<path> ();
  return 0;
}

struct curvet {
  double t, dist;
};

struct less_eq_curvet {
  static inline bool leq (curvet& a, curvet& b) { return a.dist <= b.dist; }
};

static array<curvet>
curvet_closest_points (curve c, double t1, double t2, point p, double eps) {
  array<curvet> res;
  if (t1 > t2) {
    array<curvet> r1, r2;
    r1 = curvet_closest_points (c, 0.0, t2, p, eps);
    r2 = curvet_closest_points (c, t1, 1.0, p, eps);
    res= r1 << r2;
  }
  else {
    double t;
    double closest= -1;
    point  pclosest;
    double n0        = tm_infinity;
    bool   stored    = true;
    double nprec     = n0;
    bool   decreasing= false;
    double max_step  = 0.5 / max (c->nr_components (), 1);
    for (t= t1; t <= t2;) {
      point  pt= c->evaluate (t);
      double n = norm (pt - p);
      if (n < n0) {
        n0      = n;
        closest = t;
        pclosest= pt;
        stored  = false;
      }
      decreasing= n < nprec;
      if (!stored && !decreasing) {
        curvet ct;
        ct.dist= norm (pclosest - p);
        ct.t   = closest;
        res << ct;
        stored= true;
      }
      if (stored && decreasing) n0= tm_infinity;
      double delta= (n - eps) / 2;
      t+= min (max_step, max (0.00001, c->bound (t, max (eps, delta))));
      nprec= n;
    }
    if (!stored && decreasing) {
      curvet ct;
      ct.dist= norm (pclosest - p);
      ct.t   = closest;
      res << ct;
    }
  }
  return res;
}

array<double>
curve_rep::find_closest_points (double t1, double t2, point p, double eps) {
  array<curvet> res= curvet_closest_points (curve (this), t1, t2, p, eps);
  merge_sort_leq<curvet, less_eq_curvet> (res);
  array<double> rest= array<double> (N (res));
  int           i;
  for (i= 0; i < N (res); i++)
    rest[i]= res[i].t;
  return rest;
}

double
curve_rep::find_closest_point (double t1, double t2, point p, double eps,
                               bool& found) {
  array<double> res= find_closest_points (t1, t2, p, eps);
  found            = N (res) > 0;
  if (found) return res[0];
  else return -1;
}

point
closest (curve f, point p) {
  array<double> abs;
  array<point>  pts;
  array<path>   rcip;
  f->get_control_points (abs, pts, rcip);
  if (N (abs) == 0) return f (0);
  double t1  = abs[0];
  double t2  = abs[N (abs) - 1];
  double best= 0;
  double eps = norm (f (0) - p);
  for (int i= 0; i < 10; i++) {
    bool   found= false;
    double t    = f->find_closest_point (t1, t2, p, eps, found);
    if (found) best= t;
    else break;
    double eps2= norm (f (t) - p);
    if (eps2 >= 0.9 * eps) break;
    eps= eps2;
  }
  return f (best);
}

point
grad (curve f, double t) {
  bool error= false;
  return f->grad (t, error);
}

bool
intersection (curve f, curve g, double& t, double& u) {
  // for two dimensional curves only
  double d= norm (f (t) - g (u));
  while (!fnull (d, 1.0e-9)) {
    point  ft = f (t);
    point  gu = g (u);
    point  gf = grad (f, t);
    point  gg = grad (g, u);
    double det= gf[0] * gg[1] - gf[1] * gg[0];
    if (fnull (det, 1.0e-12)) break;
    double dt= ((gu[0] - ft[0]) * gg[1] - (gu[1] - ft[1]) * gg[0]) / det;
    double du= ((gu[0] - ft[0]) * gf[1] - (gu[1] - ft[1]) * gf[0]) / det;
    double T = t + dt;
    double U = u + du;
    if (T < 0.0 || T > 1.0 || U < 0.0 || U > 1.0) break;
    double D= norm (f (T) - g (U));
    if (D > 0.9 * d) break;
    t= T;
    u= U;
    d= D;
  }
  return fnull (d, 1.0e-9);
}

array<point>
intersection (curve f, curve g, point p0, double eps) {
  // For local intersections only
  array<point> res;
  if (is_nil (f) || is_nil (g)) return res;
  bool   found= false;
  double d1, d2;
  if (f == g) {
    array<double> pts= f->find_closest_points (0.0, 1.0, p0, eps);
    if (N (pts) >= 2) {
      d1   = pts[0];
      d2   = pts[1];
      found= intersection (f, f, d1, d2);
    }
  }
  else {
    bool res1, res2;
    d1= f->find_closest_point (0.0, 1.0, p0, eps, res1);
    d2= g->find_closest_point (0.0, 1.0, p0, eps, res2);
    if (res1 && res2) found= intersection (f, g, d1, d2);
  }
  if (found) res << f (d1);
  return res;
}

/******************************************************************************
 * Segments
 ******************************************************************************/

struct segment_rep : public curve_rep {
  point p1, p2;
  path  cip1, cip2;
  segment_rep (point p1b, point p2b) : p1 (p1b), p2 (p2b) {}
  point evaluate (double t) { return (1.0 - t) * p1 + t * p2; }
  void  rectify_cumul (array<point>& a, double eps) {
    (void) eps;
    a << p2;
  }
  double bound (double t, double eps) { return curve_rep::bound (t, eps); }
  point  grad (double t, bool& error) {
    (void) t;
    error= false;
    return p2 - p1;
  }
  double curvature (double t1, double t2) {
    (void) t1;
    (void) t2;
    return tm_infinity;
  }
  int get_control_points (array<double>& abs, array<point>& pts,
                          array<path>& cip);
};

int
segment_rep::get_control_points (array<double>& abs, array<point>& pts,
                                 array<path>& cip) {
  array<double> u;
  u << 0.0;
  u << 1.0;
  abs= u;
  array<point> a;
  a << p1;
  a << p2;
  pts= a;
  cip= array<path> ();
  cip << cip1;
  cip << cip2;
  return 2;
}

curve
segment (point p1, point p2) {
  return tm_new<segment_rep> (p1, p2);
}

/******************************************************************************
 * Poly-segments
 ******************************************************************************/

struct poly_segment_rep : public curve_rep {
  array<point> a;
  array<path>  cip;
  int          n;
  poly_segment_rep (array<point> a2, array<path> cip2)
      : a (a2), cip (cip2), n (N (a) - 1) {}
  int   nr_components () { return n; }
  point evaluate (double t) {
    int i= max (min ((int) (n * t), n - 1), 0);
    return (i + 1 - n * t) * a[i] + (n * t - i) * a[i + 1];
  }
  void rectify_cumul (array<point>& cum, double eps) {
    (void) eps;
    for (int i= 0; i < n; i++)
      cum << a[i + 1];
  }
  double bound (double t, double eps) { return curve_rep::bound (t, eps); }
  double curvature (double t1, double t2) {
    (void) t1;
    (void) t2;
    return tm_infinity;
  }
  point grad (double t, bool& error) {
    error= false;
    int i= min ((int) (n * t), n - 1);
    return n * (a[i + 1] - a[i]);
  }
  int get_control_points (array<double>& abs, array<point>& pts,
                          array<path>& cip);
};

int
poly_segment_rep::get_control_points (array<double>& abs, array<point>& pts,
                                      array<path>& rcip) {
  array<double> u (n + 1);
  int           i;
  for (i= 0; i < n + 1; i++)
    u[i]= ((double) i) / (n == 0 ? 1 : n);
  abs = u;
  pts = a;
  rcip= cip;
  return n + 1;
}

curve
poly_segment (array<point> a, array<path> cip) {
  return tm_new<poly_segment_rep> (a, cip);
}

/******************************************************************************
 * Splines
 ******************************************************************************/

static const double                epsilon= 0.01; // 0.00005;
typedef polynomial<double>         dpol;
typedef vector<polynomial<double>> dpols;

struct spline_rep : public curve_rep {
  array<point>  a;
  array<path>   cip;
  int           n;
  array<double> U;
  array<dpols>  p;
  bool          close, interpol;

  spline_rep (array<point> a, array<path> cip, bool close= false,
              bool interpol= true);

  inline double d (int i, int k) { return U[i] - U[i - k]; }
  inline double m (int i) { return (U[i] + U[i + 1]) / 2; }

  double convert (double u) { return U[2] + u * (U[n + 1] - U[2]); }
  double unconvert (double u) { return (u - U[2]) / (U[n + 1] - U[2]); }
  int    interval_no (double u);

  point         spline (int i, double u, int o= 0);
  inline double S (array<dpol> p1, array<dpol> p2, array<dpol> p3, int i,
                   double u);
  point         evaluate (double t, int o);
  point         evaluate (double t);
  double        bound (double t, double eps);
  point         grad (double t, bool& error);

  double curvature (int i, double t1, double t2);
  double curvature (double t1, double t2);

  bool approx (int i, double u1, double u2, double eps);
  void rectify_cumul (array<point>& cum, int i, double u1, double u2,
                      double eps);
  void rectify_cumul (array<point>& cum, double eps);
  int  get_control_points (array<double>& abs, array<point>& pts,
                           array<path>& cip);
};

// Creation
spline_rep::spline_rep (array<point> a2, array<path> cip2, bool close2,
                        bool interpol2)
    : a (a2), cip (cip2), n (N (a) - 1), close (close2), interpol (interpol2) {
  array<dpol> p1, p2, p3;
  p1= array<dpol> (n + 3);
  p2= array<dpol> (n + 3);
  p3= array<dpol> (n + 3);
  p = array<dpols> (n + 3);
  U = array<double> (n + 6);
  if (close) {
    int i;
    a->resize (N (a) + 2);
    for (i= 0; i <= 1; i++)
      a[n + 1 + i]= a[i];
    n+= 2;
  }
  int    i;
  double x= 0;
  if (!close) {
    for (i= 0; i < 3; i++) {
      U[i]= x;
      x+= epsilon;
    }
    x+= 1 - epsilon;
    for (i= 3; i <= n; i++) {
      U[i]= x;
      x+= 1;
    }
    for (i= n + 1; i <= n + 3; i++) {
      U[i]= x;
      x+= epsilon;
    }
  }
  else {
    for (i= 0; i <= n + 3; i++) {
      U[i]= x;
      x+= 1;
    }
  }
  for (i= 0; i <= n; i++) {
    double di22= d (i + 2, 2);
    double di11= d (i + 1, 1);
    double di21= d (i + 2, 1);
    double di32= d (i + 3, 2);
    double di31= d (i + 3, 1);
    p1[i]      = dpol (square (U[i]) / di22 / di11, -2 * U[i] / di22 / di11,
                       1 / di22 / di11);
    p2[i]      = dpol (
        -U[i + 2] * U[i] / di22 / di21 - U[i + 3] * U[i + 1] / di32 / di21,
        (U[i + 2] + U[i]) / di22 / di21 + (U[i + 3] + U[i + 1]) / di32 / di21,
        -1 / di22 / di21 - 1 / di32 / di21);
    p3[i]= dpol (square (U[i + 3]) / di32 / di31, -2 * U[i + 3] / di32 / di31,
                 1 / di32 / di31);
    /*
    p1[i][2]= 1/di22/di11;
    p1[i][1]= -2*U[i]/di22/di11;
    p1[i][0]= square(U[i])/di22/di11;
    p2[i][2]= -1/di22/di21-1/di32/di21;
    p2[i][1]= (U[i+2]+U[i])/di22/di21+(U[i+3]+U[i+1])/di32/di21;
    p2[i][0]= -U[i+2]*U[i]/di22/di21-U[i+3]*U[i+1]/di32/di21;
    p3[i][2]= 1/di32/di31;
    p3[i][1]= -2*U[i+3]/di32/di31;
    p3[i][0]= square(U[i+3])/di32/di31;
    */
  }
  if (interpol) {
    array<point> x (n + 1), y (n + 1);
    y= a;
    {
      array<double> a (n + 1), b (n + 1), c (n + 1);
      if (close) {
        a[n - 2]= S (p1, p2, p3, n - 3, m (n - 1));
        b[0]    = S (p1, p2, p3, 1, m (2));
        b[n - 2]= S (p1, p2, p3, n - 2, m (n - 1));
        c[0]    = S (p1, p2, p3, 2, m (2));
        for (i= 1; i < n - 2; i++) {
          a[i]= S (p1, p2, p3, i, m (i + 2));
          b[i]= S (p1, p2, p3, i + 1, m (i + 2));
          c[i]= S (p1, p2, p3, i + 2, m (i + 2));
        }
        xtridiag_solve (a, b, c, S (p1, p2, p3, n - 1, m (n - 1)),
                        S (p1, p2, p3, 0, m (2)), x, y, n - 1);
        /* Rotate the result in order to have an appropriate
           parametrization (to keep in sync the ordering of
           the points & the paths in cip, in particular) */
        point p= x[n - 2];
        for (i= n - 2; i >= 1; i--)
          x[i]= x[i - 1];
        x[0]= p;
        // Splice the spline
        x[n - 1]= x[0];
        x[n]    = x[1];
      }
      else {
        a[n]= S (p1, p2, p3, n - 1, U[n + 1]);
        b[0]= S (p1, p2, p3, 0, U[2]);
        b[n]= S (p1, p2, p3, n, U[n + 1]);
        c[0]= S (p1, p2, p3, 1, U[2]);
        for (i= 1; i < n; i++) {
          a[i]= S (p1, p2, p3, i - 1, m (i + 1));
          b[i]= S (p1, p2, p3, i, m (i + 1));
          c[i]= S (p1, p2, p3, i + 1, m (i + 1));
        }
        tridiag_solve (a, b, c, x, y, n + 1);
      }
    }
    a= x;
  }
  for (i= 2; i <= n; i++)
    p[i]= a[i] * p1[i] + a[i - 1] * p2[i - 1] + a[i - 2] * p3[i - 2];
}

// Evaluation
double
spline_rep::S (array<dpol> p1, array<dpol> p2, array<dpol> p3, int i,
               double u) {
  if (i < 0 || i > n) return 0;
  else if (u < U[i] || u >= U[i + 3]) return 0;
  else if (u < U[i + 1]) return p1[i](u);
  else if (u < U[i + 2]) return p2[i](u);
  else if (u < U[i + 3]) return p3[i](u);
  else TM_FAILED ("we should **never** go here");
  return 0.0;
}

point
spline_rep::spline (int i, double u, int o) {
  int   j, n= N (p[i]);
  point res (n);
  if (o < 0 || o > 2) o= 0;
  for (j= 0; j < n; j++)
    res[j]= p[i][j](u, o);
  return res;
}

int
spline_rep::interval_no (double u) {
  int i;
  for (i= 0; i < N (U); i++)
    if (u >= U[i] && u < U[i + 1]) return i;
  return -1;
}

static double
prod (double x, int n) {
  double r;
  if (n <= 0) return 1;
  r= x;
  n--;
  while (n--)
    r*= (x - n);
  return r;
}

point
spline_rep::evaluate (double t, int o) {
  point res;
  int   no;
  t       = convert (t);
  double k= U[n + 1] - U[2];
  no      = interval_no (t);
  if (no < 2) res= spline (2, U[2], o);
  else if (no > n) res= spline (n, U[n + 1], o);
  else res= spline (no, t, o);
  return prod (k, o) * res;
}

point
spline_rep::evaluate (double t) {
  return evaluate (t, 0);
}

double
spline_rep::bound (double t, double eps) {
  return eps / norm (evaluate (t, 1));
}

point
spline_rep::grad (double t, bool& error) {
  error= false;
  return evaluate (t, 1);
}

// Rectification
bool
spline_rep::approx (int i, double u1, double u2, double eps) {
  double l, R;
  point  p1, p2;
  p1= spline (i, u1);
  p2= spline (i, u2);
  l = norm (p1 - p2);
  // When l and R are very small, the test l<=R
  // can fail forever. So we set l to exactly 0
  if (l != 0 && fnull (l, 1.0e-6)) l= 0;
  R= curvature (i, u1, u2);
  return l <= 2 * sqrt (2 * R * eps);
}

void
spline_rep::rectify_cumul (array<point>& cum, int i, double u1, double u2,
                           double eps) {
  if (approx (i, u1, u2, eps)) cum << spline (i, u2);
  else {
    double u= (u1 + u2) / 2;
    rectify_cumul (cum, i, u1, u, eps);
    rectify_cumul (cum, i, u, u2, eps);
  }
}

void
spline_rep::rectify_cumul (array<point>& cum, double eps) {
  int i;
  for (i= 2; i <= n; i++)
    rectify_cumul (cum, i, U[i], U[i + 1], eps);
}

// Curvature
double
spline_rep::curvature (int i, double t1, double t2) {
  point a, b;
  a= extract (p[i], 2);
  b= extract (p[i], 1);
  double t, R;
  point  pp, ps;
  if (norm (a) == 0) return tm_infinity;
  t= -inner (a, b) / (2 * inner (a, a));
  if (t1 > t) t= t1;
  else if (t2 < t) t= t2;
  pp= spline (i, t, 1);
  ps= spline (i, t, 2);
  if (norm (ps) == 0) return tm_infinity;
  R= square (norm (pp)) / norm (ps);
  return R;
}

double
spline_rep::curvature (double t1, double t2) {
  double res;
  int    no, no1, no2;
  t1 = convert (t1);
  t2 = convert (t2);
  no1= interval_no (t1);
  no2= interval_no (t2);
  res= tm_infinity;
  for (no= no1; no <= no2; no++) {
    double r;
    if (no < 2) r= curvature (2, t1, t2);
    else if (no > n) r= curvature (n, t1, t2);
    else r= curvature (no, t1, t2);
    if (r <= res) res= r;
  }
  return res;
}

// Control points
int
spline_rep::get_control_points (array<double>& abs, array<point>& pts,
                                array<path>& rcip) {
  array<double> u (n + 1);
  array<point>  p (n + 1);
  int           i;
  if (interpol) {
    if (close) {
      u->resize (n - 1);
      p->resize (n - 1);
      for (i= 0; i < n - 1; i++)
        u[i]= unconvert ((U[i + 2] + U[i + 3]) / 2);
    }
    else {
      u[0]= unconvert (U[2]);
      u[n]= unconvert (U[n + 1]);
      for (i= 2; i <= n; i++)
        u[i - 1]= unconvert ((U[i] + U[i + 1]) / 2);
    }
    for (i= 0; i <= (close ? n - 2 : n); i++)
      p[i]= evaluate (u[i]);
  }
  else TM_FAILED ("not yet implemented");
  abs = u;
  pts = p;
  rcip= cip;
  return close ? n - 1 : n + 1;
}

curve
spline (array<point> a, array<path> cip, bool close, bool interpol) {
  return tm_new<spline_rep> (a, cip, close, interpol);
}

/******************************************************************************
 * Bezier curves
 ******************************************************************************/

struct bezier_rep : public curve_rep {
  array<point> a;
  array<point> P;
  bezier_rep (array<point> a);
  point  evaluate (double t);
  void   rectify_cumul (array<point>& cum, double t0, double t1, double eps);
  void   rectify_cumul (array<point>& cum, double eps);
  double bound (double t, double eps);
  point  grad (double t, bool& error);
  double curvature (double t1, double t2);
};

bezier_rep::bezier_rep (array<point> a2) : a (a2) {
  P << a[0];
  P << (-3.0 * a[0] + 3.0 * a[1]);
  P << (3.0 * a[0] - 6.0 * a[1] + 3.0 * a[2]);
  P << (-a[0] + 3.0 * a[1] - 3.0 * a[2] + a[3]);
}

point
bezier_rep::evaluate (double t) {
  return ((P[3] * t + P[2]) * t + P[1]) * t + P[0];
}

void
bezier_rep::rectify_cumul (array<point>& cum, double t0, double t1, double e) {
  point p0= evaluate (t0);
  point p1= evaluate (t1);
  // if (bound ((t0 + t1) / 2.0, e / 4.0) < (t1 - t0))
  for (int k= 1; k <= 4; k++) {
    double x= ((double) k) / 5.0;
    double t= (1.0 - x) * t0 + x * t1;
    point  q= evaluate (t);
    point  r= (1.0 - x) * p0 + x * p1;
    if (norm (q - r) >= (e / 10.0)) {
      rectify_cumul (cum, t0, (t0 + t1) / 2.0, e);
      rectify_cumul (cum, (t0 + t1) / 2.0, t1, e);
      return;
    }
  }
  cum << p1;
}

void
bezier_rep::rectify_cumul (array<point>& cum, double eps) {
  rectify_cumul (cum, 0.0, 1.0, eps);
}

double
bezier_rep::bound (double t, double eps) {
  (void) t;
  double K= 3.0 * norm (P[3]) + 2.0 * norm (P[2]) + norm (P[1]);
  return min (eps / (K + 1.0e-6), 1.0);
}

point
bezier_rep::grad (double t, bool& error) {
  error= false;
  return ((3.0 * P[3] * t) + 2.0 * P[2]) + P[1];
}

double
bezier_rep::curvature (double t1, double t2) {
  (void) t1;
  (void) t2;
  return tm_infinity; // FIXME
}

curve
bezier (array<point> a) {
  return tm_new<bezier_rep> (a);
}

/******************************************************************************
 * Poly-Bezier curves
 ******************************************************************************/

struct poly_bezier_rep : public curve_rep {
  double       phi;
  array<point> a;
  array<path>  cip;
  curve        wrap;
  bool         simple;
  bool         closed;
  poly_bezier_rep (array<point> a, array<path> cip, bool simple, bool closed);
  point  evaluate (double t);
  void   rectify_cumul (array<point>& cum, double eps);
  double bound (double t, double eps);
  point  grad (double t, bool& error);
  double curvature (double t1, double t2);
  int    get_control_points (array<double>& abs, array<point>& pts,
                             array<path>& cip);
};

poly_bezier_rep::poly_bezier_rep (array<point> a2, array<path> cip2,
                                  bool simple2, bool closed2)
    : phi (3.0), a (a2), cip (cip2), simple (simple2), closed (closed2) {
  array<point> p;
  if (simple) {
    array<point> b;
    int          di= (closed ? 0 : 1);
    for (int i= di; i < N (a) - di; i++) {
      point p0= a[(i + N (a) - 1) % N (a)];
      point p1= a[i];
      point p2= a[(i + 1) % N (a)];
      if (norm (p2 - p0) < 1.0e-6) {
        b << (p0 + (phi - 1.0) * p1) / phi;
        b << p1;
        b << ((phi - 1.0) * p1 + p2) / phi;
      }
      else {
        point  u = (p2 - p0) / norm (p2 - p0);
        double l1= norm (p1 - p0) / phi;
        double l2= norm (p2 - p1) / phi;
        b << p1 - l1 * u;
        b << p1;
        b << p1 + l2 * u;
      }
    }
    if (closed) {
      p << range (b, 1, N (b));
      p << range (b, 0, 2);
    }
    else if (N (a) == 2) {
      p << a[0];
      p << ((phi - 1.0) * a[0] + a[1]) / phi;
      p << (a[0] + (phi - 1.0) * a[1]) / phi;
      p << a[1];
    }
    else {
      p << a[0];
      if (norm (b[0] - a[0]) < 1.0e-6) p << b[0];
      else {
        double l1= norm (a[1] - a[0]) / phi;
        double l2= norm (b[0] - a[0]);
        p << a[0] + (b[0] - a[0]) * (l1 / l2);
      }
      p << b;
      if (norm (a[N (a) - 1] - b[N (b) - 1]) < 1.0e-6) p << b[N (b) - 1];
      else {
        double l1= norm (a[N (a) - 1] - a[N (a) - 2]) / phi;
        double l2= norm (b[N (b) - 1] - a[N (a) - 1]);
        p << a[N (a) - 1] + (b[N (b) - 1] - a[N (a) - 1]) * (l1 / l2);
      }
      p << a[N (a) - 1];
    }
  }
  else {
    p= copy (a);
    if (closed) p << a[0];
    if (((N (p) - 1) % 3) == 1) {
      p << p[N (p) - 1];
      p[N (p) - 2]= p[N (p) - 3] + (p[N (p) - 3] - p[N (p) - 4]);
    }
    if (((N (p) - 1) % 3) == 2) {
      p << p[N (p) - 1];
      p[N (p) - 2]= p[N (p) - 3];
    }
  }
  array<curve> cs;
  for (int i= 0; (i + 3) < N (p); i+= 3) {
    array<point> b;
    b << p[i] << p[i + 1] << p[i + 2] << p[i + 3];
    cs << bezier (b);
  }
  wrap= compound (cs);
}

point
poly_bezier_rep::evaluate (double t) {
  return wrap->evaluate (t);
}

void
poly_bezier_rep::rectify_cumul (array<point>& cum, double eps) {
  wrap->rectify_cumul (cum, eps);
}

double
poly_bezier_rep::bound (double t, double eps) {
  return wrap->bound (t, eps);
}

point
poly_bezier_rep::grad (double t, bool& error) {
  return wrap->grad (t, error);
}

double
poly_bezier_rep::curvature (double t1, double t2) {
  return wrap->curvature (t1, t2);
}

int
poly_bezier_rep::get_control_points (array<double>& abs, array<point>& pts,
                                     array<path>& rcip) {
  abs = array<double> ();
  pts = a;
  rcip= cip;
  if (simple) {
    int n= N (a);
    int k= (closed ? n : n - 1);
    for (int i= 0; i < n; i++)
      abs << ((double) i) / ((double) k);
  }
  else {
    int n= N (a);
    if (!closed) n--;
    int k= 3 * ((n + 2) / 3);
    for (int i= 0; i < n; i++)
      abs << ((double) i) / ((double) k);
    if (!closed) abs << 1.0;
  }
  return N (a);
}

curve
poly_bezier (array<point> a, array<path> cip, bool simple, bool closed) {
  return tm_new<poly_bezier_rep> (a, cip, simple, closed);
}

/******************************************************************************
 * Arcs
 ******************************************************************************/

struct arc_rep : public curve_rep {
  array<point>  a;
  array<path>   cip;
  array<double> u;
  point         center; // Center
  point         i, j;   // The two base vectors of the ellipsis's 2D plane
  double        r1, r2; // The two radiuses of the ellipsis
  double        e1, e2; // Coordinates of the two extremal points of the arc
  arc_rep (array<point> a, array<path> cip, bool close);
  point  evaluate (double t);
  void   rectify_cumul (array<point>& cum, double eps);
  double bound (double t, double eps);
  point  grad (double t, bool& error);
  double curvature (double t1, double t2);
  int    get_control_points (array<double>& abs, array<point>& pts,
                             array<path>& cip);
};

arc_rep::arc_rep (array<point> a2, array<path> cip2, bool close)
    : a (a2), cip (cip2) {
  int   n = N (a);
  point o1= (n > 0 ? a[0] : point (0, 0));
  point o2= (n > 1 ? a[1] : point (0, 0));
  point o3= (n > 2 ? a[2] : point (0, 0));
  if (n != 3 || linearly_dependent (o1, o2, o3) ||
      (N (center= intersection (midperp (o1, o2, o3), midperp (o2, o3, o1))) ==
       0)) {
    center= i= j= 0 * o1;
    r1= r2= 1;
    e1    = 0;
    e2    = 1;
    a     = array<point> (1);
    a[0]  = o1;
    if (N (cip)) {
      path p= cip[0];
      cip   = array<path> (1);
      cip[0]= p;
    }
    u   = array<double> (1);
    u[0]= 0;
    return;
  }
  r1= r2= norm (o1 - center);
  if (orthogonalize (i, j, center, o1, o2))
    ;
  else orthogonalize (i, j, center, o1, o3);
  e1= 0;
  point o2b (2), o3b (2);
  o2b[0]   = inner (o2 - center, i);
  o2b[1]   = inner (o2 - center, j);
  o3b[0]   = inner (o3 - center, i);
  o3b[1]   = inner (o3 - center, j);
  e2       = arg (o3b) / (2 * tm_PI);
  double e3= arg (o2b) / (2 * tm_PI);
  if (e2 < e3) {
    j     = -j;
    o3b[1]= -o3b[1];
    e2    = arg (o3b) / (2 * tm_PI);
    o2b[1]= -o2b[1];
    e3    = arg (o2b) / (2 * tm_PI);
  }
  u   = array<double> (3);
  u[0]= 0;
  u[1]= e3;
  u[2]= e2;
  if (close) e2= 1.0;
}

point
arc_rep::evaluate (double t) {
  t= e1 + t * (e2 - e1);
  return center + r1 * cos (2 * tm_PI * t) * i + r2 * sin (2 * tm_PI * t) * j;
}

void
arc_rep::rectify_cumul (array<point>& cum, double eps) {
  double t, step;
  step= sqrt (2 * eps / max (r1, r2)) / tm_PI;
  for (t= step; t <= 1.0; t+= step)
    cum << evaluate (t);
  if (t - step != 1.0) cum << evaluate (1.0);
}

double
arc_rep::bound (double t, double eps) {
  return curve_rep::bound (t, eps);
}

point
arc_rep::grad (double t, bool& error) {
  error= false;
  t    = e1 + t * (e2 - e1);
  return -2 * tm_PI * r1 * sin (2 * tm_PI * t) * i +
         2 * tm_PI * r2 * cos (2 * tm_PI * t) * j;
}

double
arc_rep::curvature (double t1, double t2) {
  (void) t1;
  (void) t2;
  if (r1 >= r2) return fnull (r1, 1.0e-6) ? tm_infinity : square (r2) / r1;
  else return fnull (r2, 1.0e-6) ? tm_infinity : square (r1) / r2;
}

curve
arc (array<point> a, array<path> cip, bool close) {
  return tm_new<arc_rep> (a, cip, close);
}

int
arc_rep::get_control_points (array<double>& abs, array<point>& pts,
                             array<path>& rcip) {
  abs = u;
  pts = a;
  rcip= cip;
  return N (a);
}

/******************************************************************************
 * Ovals
 ******************************************************************************/

struct ellipse_rep : public curve_rep {
  array<point> points;
  array<path>  cip;
  point        f1, f2;       // two foci of the ellipsis
  point        center;       // the center of the ellipse
  double       focal_length; // the distance between f1 and f2
  double sum_of_two_dis; // The sum of the distances of any point on the ellipse
                         // to f1 and f2
  point  i, j;           // The two base vectors of the ellipsis's 2D plane
  double r1, r2;         // The two radiuses of the ellipsis
  ellipse_rep (array<point> a, array<path> cip, bool close);
  point  evaluate (double t) override;
  void   rectify_cumul (array<point>& cum, double eps) override;
  double bound (double t, double eps) override;
  point  grad (double t, bool& error) override;
  double curvature (double t1, double t2) override;
  int    get_control_points (array<double>& abs, array<point>& pts,
                             array<path>& cip) override;
};

ellipse_rep::ellipse_rep (array<point> a2, array<path> cip2, bool close)
    : points (a2), cip (cip2) {
  int n= N (points);
  // we must ensure that the input "n" is 3.
  // Point 0 and Point 1 are the two foci of the ellipse. Point 2 is an
  // arbitrary point on the ellipse.
  ASSERT (n == 3, "WRONG PARAMETERS OF ELLIPSE");

  f1            = points[0];
  f2            = points[1];
  center        = (f1 + f2) / 2;
  focal_length  = norm (f2 - f1);
  sum_of_two_dis= norm (points[2] - f1) + norm (points[2] - f2);
  r1            = (sum_of_two_dis) / 2;
  r2= sqrt (square (sum_of_two_dis / 2) - square (focal_length / 2));
  if (orthogonalize (i, j, center, points[0], points[1]))
    ;
  else orthogonalize (i, j, center, points[0], points[2]);
}

point
ellipse_rep::evaluate (double t) {
  return center + r1 * cos (2 * tm_PI * t) * i + r2 * sin (2 * tm_PI * t) * j;
}

void
ellipse_rep::rectify_cumul (array<point>& cum, double eps) {
  double t, step;
  step= sqrt (2 * eps / max (r1, r2)) / tm_PI;
  for (t= step; t <= 1.0; t+= step)
    cum << evaluate (t);
  if (t - step != 1.0) cum << evaluate (1.0);
}

double
ellipse_rep::bound (double t, double eps) {
  return curve_rep::bound (t, eps);
}

point
ellipse_rep::grad (double t, bool& error) {
  error= false;
  return -2 * tm_PI * r1 * sin (2 * tm_PI * t) * i +
         2 * tm_PI * r2 * cos (2 * tm_PI * t) * j;
}

double
ellipse_rep::curvature (double t1, double t2) {
  (void) t1;
  (void) t2;
  if (r1 >= r2) return fnull (r1, 1.0e-6) ? tm_infinity : square (r2) / r1;
  else return fnull (r2, 1.0e-6) ? tm_infinity : square (r1) / r2;
}

curve
ellipse (array<point> a, array<path> cip, bool close) {
  return tm_new<ellipse_rep> (a, cip, close);
}

int
ellipse_rep::get_control_points (array<double>& abs, array<point>& pts,
                                 array<path>& rcip) {
  // According to the design, the points in the "abs" array represent the
  // control points of the curve, with their corresponding parameter values on
  // the curve. However, in practice, only the first and last points in the
  // array will be used. Therefore, the "abs" array is used to represent the
  // range of parameter values. Since we have a closed curve here, we just need
  // to ensure that the starting point has a parameter value of 0 and the ending
  // point has a parameter value of 1.
  abs = array<double> (0.0, 1.0);
  pts = points;
  rcip= cip;
  return N (points);
}

/******************************************************************************
 * Compound curves
 ******************************************************************************/

struct compound_curve_rep : public curve_rep {
  array<curve> cs;
  compound_curve_rep (array<curve> cs2) : cs (cs2) {}
  int nr_components () {
    int tot= 0;
    for (int i= 0; i < N (cs); i++)
      tot+= cs[i]->nr_components ();
    return tot;
  }
  int get_index (double t) {
    int i= (int) floor (N (cs) * t);
    if (i < 0) return 0;
    if (i >= N (cs)) return N (cs) - 1;
    return i;
  }
  double get_subtime (double t) {
    int    i  = (int) floor (N (cs) * t);
    double res= (N (cs) * t) - floor (N (cs) * t);
    if (i < 0) return 0.0;
    if (i >= N (cs)) return 1.0;
    return res;
  }
  point evaluate (double t) {
    return cs[get_index (t)]->evaluate (get_subtime (t));
  }
  void rectify_cumul (array<point>& a, double eps) {
    for (int i= 0; i < N (cs); i++)
      cs[i]->rectify_cumul (a, eps);
  }
  double bound (double t, double eps) {
    double st= get_subtime (t);
    double dt= cs[get_index (t)]->bound (st, eps);
    dt       = min (min (dt, st + 1.0e-6), 1.0 + 1.0e-6 - st);
    return dt / N (cs);
  }
  point grad (double t, bool& error) {
    int   i= get_index (t);
    point g= cs[i]->grad (get_subtime (t), error);
    if (get_subtime (t) <= 1.0e-6 && i > 0) {
      point g2= cs[i - 1]->grad (1.0, error);
      if (norm (g2 - g) >= 1.0e-6) error= true;
    }
    return N (cs) * g;
  }
  double curvature (double t1, double t2) {
    int    i1= get_index (t1);
    int    i2= get_index (t2);
    double u1= get_subtime (t1);
    double u2= get_subtime (t2);
    double m = 0.0;
    if (i1 == i2) m= cs[i1]->curvature (u1, u2);
    else
      for (int i= i1; i <= i2; i++) {
        if (i == i1) m= max (m, cs[i]->curvature (u1, 1.0));
        else if (i == i2) m= max (m, cs[i]->curvature (0.0, u2));
        else m= max (m, cs[i]->curvature (0.0, 1.0));
      }
    return m;
  }
};

curve
compound (array<curve> a) {
  // FIXME: we might want to test for continuity at the junctions
  if (N (a) == 0) TM_FAILED ("at least one curve expected");
  if (N (a) == 1) return a[0];
  return tm_new<compound_curve_rep> (a);
}

/******************************************************************************
 * Inverted curves
 ******************************************************************************/

struct inverted_curve_rep : public curve_rep {
  curve c;
  int   n;
  inverted_curve_rep (curve c2) : c (c2), n (c->nr_components ()) {}
  int   nr_components () { return n; }
  point evaluate (double t) { return c (1.0 - t); }
  void  rectify_cumul (array<point>& a, double eps) {
    array<point> b= c->rectify (eps);
    int          i, k= N (b);
    for (i= k - 1; i >= 0; i--)
      a << b[i];
  }
  double bound (double t, double eps) { return curve_rep::bound (t, eps); }
  point  grad (double t, bool& error) { return -c->grad (1.0 - t, error); }
  double curvature (double t1, double t2) {
    return c->curvature (1 - t2, 1 - t1);
  }
  int get_control_points (array<double>& abs, array<point>& pts,
                          array<path>& cip);
};

int
inverted_curve_rep::get_control_points (array<double>& abs, array<point>& pts,
                                        array<path>& cip) {
  int res= c->get_control_points (abs, pts, cip);
  int i;
  abs= copy (abs);
  for (i= 0; i < res; i++)
    abs[i]= 1 - abs[i];
  return res;
}

curve
invert (curve c) {
  return tm_new<inverted_curve_rep> (c);
}

/******************************************************************************
 * Transformed curves
 ******************************************************************************/

struct transformed_curve_rep : public curve_rep {
  frame f;
  curve c;
  int   n;
  transformed_curve_rep (frame f2, curve c2)
      : f (f2), c (c2), n (c->nr_components ()) {}
  int    nr_components () { return n; }
  point  evaluate (double t) { return f (c (t)); }
  void   rectify_cumul (array<point>& a, double eps);
  double bound (double t, double eps) { return curve_rep::bound (t, eps); }
  point  grad (double t, bool& error);
  double curvature (double t1, double t2) {
    (void) t1;
    (void) t2;
    TM_FAILED ("not yet implemented");
    return 0.0;
  }
  int get_control_points (array<double>& abs, array<point>& pts,
                          array<path>& cip);
};

void
transformed_curve_rep::rectify_cumul (array<point>& a, double eps) {
  if (f->linear) {
    double       delta= f->direct_bound (c (0.0), eps);
    array<point> b    = c->rectify (delta);
    int          i, k= N (b);
    for (i= 0; i < k; i++)
      a << f (b[i]);
  }
  else TM_FAILED ("not yet implemented");
}

point
transformed_curve_rep::grad (double t, bool& error) {
  bool  error2;
  point w2= c->grad (t, error2);
  point w1= f->jacobian (c (t), w2, error);
  error|= error2;
  return w1;
}

int
transformed_curve_rep::get_control_points (array<double>& abs,
                                           array<point>&  pts,
                                           array<path>&   cip) {
  int res= c->get_control_points (abs, pts, cip);
  int i;
  pts= copy (pts);
  for (i= 0; i < N (pts); i++)
    pts[i]= f (pts[i]);
  return res;
}

curve
frame::operator() (curve c) {
  return tm_new<transformed_curve_rep> (*this, c);
}

curve
frame::operator[] (curve c) {
  return tm_new<transformed_curve_rep> (invert (*this), c);
}

/******************************************************************************
 * Portions of curves
 ******************************************************************************/

void
poor_rectify_cumul (curve c, array<point>& a, double eps, array<double>& ts,
                    double t0, double t1) {
  int   NUM= 5;
  point p0 = c->evaluate (t0);
  point p1 = c->evaluate (t1);
  for (int i= 1; i < NUM; i++) {
    double u  = ((double) i) / ((double) NUM);
    point  pi = c->evaluate (t0 + u * (t1 - t0));
    point  pi2= p0 + u * (p1 - p0);
    if (norm (pi2 - pi) >= eps) {
      poor_rectify_cumul (c, a, eps, ts, t0, (t0 + t1) / 2);
      poor_rectify_cumul (c, a, eps, ts, (t0 + t1) / 2, t1);
      return;
    }
  }
  a << p1;
  ts << t1;
}

struct partial_curve_rep : public curve_rep {
  curve  c;
  double t0, t1;
  partial_curve_rep (curve c2, double t0b, double t1b)
      : c (c2), t0 (t0b), t1 (t1b) {}
  int   nr_components () { return c->nr_components (); }
  point evaluate (double t) { return c (t0 + (t1 - t0) * t); }
  void  rectify_cumul (array<point>& a, double eps) {
    array<double> ts;
    poor_rectify_cumul (c, a, eps, ts, t0, t1);
  }
  double bound (double t, double eps) {
    return c->bound (t0 + (t1 - t0) * t, eps) / (t1 - t0);
  }
  point grad (double t, bool& error) {
    return (t1 - t0) * c->grad (t0 + (t1 - t0) * t, error);
  }
  double curvature (double u1, double u2) {
    return c->curvature (t0 + (t1 - t0) * u1, t0 + (t1 - t0) * u2);
  }
  int           get_control_points (array<double>& abs, array<point>& pts,
                                    array<path>& cip);
  array<double> find_closest_points (double t1, double t2, point p, double eps);
};

int
partial_curve_rep::get_control_points (array<double>& abs, array<point>& pts,
                                       array<path>& cip) {
  int res= c->get_control_points (abs, pts, cip);
  int i;
  abs= copy (abs);
  for (i= 0; i < res; i++)
    abs[i]= (abs[i] - t0) / (t1 - t0);
  return res;
}

array<double>
partial_curve_rep::find_closest_points (double t1, double t2, point p,
                                        double eps) {
  if (t1 > t2) {
    double a= t1;
    t1      = t2;
    t2      = a;
  }
  return curve_rep::find_closest_points (t1, t2, p, eps);
}

curve
part (curve c, double start, double end) {
  ASSERT (fabs (end - start) > 1.0e-6, "very small portions not supported");
  return tm_new<partial_curve_rep> (c, start, end);
}

curve
truncate (curve c, double portion, double eps) {
  if (portion == 1.0) return c;
  ASSERT (portion > 1.0e-6 && portion < 1, "invalid portion of curve");
  array<point>  a;
  array<double> ts;
  a << c->evaluate (0.0);
  ts << 0.0;
  poor_rectify_cumul (c, a, eps, ts, 0.0, 1.0);
  double tot= 0.0;
  for (int i= 1; i < N (a); i++)
    tot+= norm (a[i] - a[i - 1]);
  double rem= portion * tot;
  for (int i= 1; i < N (a); i++) {
    double d= norm (a[i] - a[i - 1]);
    if (d > rem) {
      double t0= ts[i - 1];
      double t1= ts[i];
      double a = rem / d;
      double tt= t0 + a * (t1 - t0);
      return part (c, 0.0, tt);
    }
    else rem-= d;
  }
  return c;
}

/******************************************************************************
 * Changing the control points
 ******************************************************************************/

struct recontrol_curve_rep : public curve_rep {
  curve        c;
  array<point> a;
  array<path>  cip;
  recontrol_curve_rep (curve c2, array<point> a2, array<path> cip2)
      : c (c2), a (a2), cip (cip2) {}
  int    nr_components () { return c->nr_components (); }
  point  evaluate (double t) { return c (t); }
  void   rectify_cumul (array<point>& a, double e) { c->rectify_cumul (a, e); }
  double bound (double t, double eps) { return c->bound (t, eps); }
  point  grad (double t, bool& error) { return c->grad (t, error); }
  double curvature (double u1, double u2) { return c->curvature (u1, u2); }
  int    get_control_points (array<double>& abs, array<point>& a2,
                             array<path>& cip2) {
    a2  = a;
    cip2= cip;
    int i, n= N (a);
    abs= array<double> (n);
    for (i= 0; i < n; i++)
      abs[i]= i / (1.0 * max (1, n - 1));
    return n;
  }
  array<double> find_closest_points (double t1, double t2, point p,
                                     double eps) {
    return c->find_closest_points (t1, t2, p, eps);
  }
};

curve
recontrol (curve c, array<point> a, array<path> cip) {
  return tm_new<recontrol_curve_rep> (c, a, cip);
}
